#include "fix_queue.h"
#include "net_err.h"
#include "stdtypes.h"
#include "exmsg.h"

#ifdef USE_ATOMIC
// #define TAKE_QUEUE(q) \
//     do { atomic_load(q->write_enable); } while ((*q->write_enable) == 0); \
//     atomic_store(q->write_enable, 0)

// #define RELEASE_QUEUE(q) \
//     atomic_store(q->write_enable, 1)
#else
#define TAKE_QUEUE(m) sys_mutex_lock(m);
#define RELEASE_QUEUE(m) sys_mutex_unlock(m);
#endif

#if 0
net_err_t fix_queue_init(fix_queue_t *q, void **buf, u32 size) {
    q->size = size;
    q->in = q->out = q->cnt = 0;
    q->buf = buf;
    q->suspend_w = (nlist_node_t *)malloc(sizeof(nlist_node_t));
    nlist_node_init(q->suspend_w);
    q->suspend_r = (nlist_node_t *)malloc(sizeof(nlist_node_t));
    nlist_node_init(q->suspend_r);
    q->qmutex = sys_mutex_create();
    if (q->qmutex == SYS_MUTEX_INVALID) {
        LOG_FATAL("queue mutex create fail\n");
        return NET_ERR_SYS;
    }
    LOG_DEBUG("queue size = %d, queue cnt = %d, queue in = %d, queue out = %d\n", q->size, q->cnt, q->in, q->out);
    LOG_DEBUG("queue init success\n");
    return NET_ERR_OK;
}

static int fix_queue_wait_write(fix_queue_t *q, int time_out) {
    sys_thread_t t = GetCurrentThread();
    queue_suspend_list *new_list = (queue_suspend_list *)malloc(sizeof(queue_suspend_list));
    new_list->suspend_thread = t;
    nlist_node_set_next(q->suspend_w, new_list->suspend_list);
    net_time_t tim;
    sys_time_curr(&tim);
    sys_sleep(time_out);
    // SuspendThread(t);
    if (sys_time_goes(&tim) <= time_out) {
        return 1;
    }
    return -1;
}

static void fix_queue_notify_write(fix_queue_t *q) {
    if (q->suspend_w->next == q->suspend_w) {
        return;
    }
    nlist_node_t *next_node = nlist_node_next(q->suspend_w);
    sys_thread_t  th = mnet_get_list_entry(next_node, queue_suspend_list, suspend_list)->suspend_thread;
    ResumeThread(th);
    nlist_node_remove(next_node);
    free(next_node);
}

static int fix_queue_wait_read(fix_queue_t *q, int time_out) {
    sys_thread_t t = GetCurrentThread();
    queue_suspend_list *new_list = (queue_suspend_list *)malloc(sizeof(queue_suspend_list));
    new_list->suspend_thread = t;
    LOG_DEBUG("current thread: %x\n", t);
    nlist_node_set_next(q->suspend_r, new_list->suspend_list);
    net_time_t tim;
    sys_time_curr(&tim);
    LOG_DEBUG("current time: %x\n", tim);
    sys_sleep(time_out);
    // SuspendThread(t);
    LOG_DEBUG("thread resume\n");
    if (sys_time_goes(&tim) <= time_out) {
        return 1;
    }
    return -1;
}

static void fix_queue_notify_read(fix_queue_t *q) {
    if (q->suspend_r->next == q->suspend_r) {
        return;
    }
    nlist_node_t *next_node = nlist_node_next(q->suspend_r);
    sys_thread_t  th = mnet_get_list_entry(next_node, queue_suspend_list, suspend_list)->suspend_thread;
    ResumeThread(th);
    nlist_node_remove(next_node);
    free(next_node);
}
#endif

net_err_t fix_queue_init(fix_queue_t *q, void **buf, u32 size) {
    q->size = size;
    q->in = q->out = q->cnt = 0;
    q->buf = buf;
    q->recv_wait_cnt = 0;
    q->send_wait_cnt = 0;
    q->write_enable = sys_event_create(NULL);
    q->read_enable = sys_event_create(NULL);
    q->qmutex = sys_mutex_create();
    if (q->qmutex == SYS_MUTEX_INVALID || q->write_enable == SYS_EVENT_INVALID || q->read_enable == SYS_EVENT_INVALID) {
        LOG_FATAL("queue resource create fail\n");
        return NET_ERR_SYS;
    }
    LOG_DEBUG("queue size = %d, queue cnt = %d, queue in = %d, queue out = %d\n", q->size, q->cnt, q->in, q->out);
    LOG_DEBUG("queue init success\n");
    return NET_ERR_OK;
}

void * fix_queue_recv(fix_queue_t *q, int time_out) {
    sys_mutex_lock(q->qmutex);
    LOG_DEBUG("[recv]msg queue size:%d, msg queue cnt:%d\n", q->size, q->cnt);
    if (q->cnt <= 0) {
        if (time_out == 0) {
            LOG_DEBUG("queue data null\n");
            sys_mutex_unlock(q->qmutex);
            return (void *)0;
        }
        q->recv_wait_cnt++;
        sys_mutex_unlock(q->qmutex);
        if (sys_event_wait(q->read_enable, time_out) < 0) {
            sys_mutex_lock(q->qmutex);
            q->recv_wait_cnt--;
            LOG_DEBUG("recv timeout\n");
            sys_mutex_unlock(q->qmutex);
            return (void *)0;
        }
        sys_mutex_lock(q->qmutex);
        q->recv_wait_cnt--;
        LOG_DEBUG("[recv prev]msg cnt = %d\n", q->cnt);
    }

    void * msg = q->buf[q->out++];
    if (q->out >= q->size) {
        q->out = 0;
    }
    q->cnt--;
    LOG_DEBUG("current send wait cnt:%d\n", q->send_wait_cnt);
    if (q->send_wait_cnt) sys_event_notify(q->write_enable);
    sys_mutex_unlock(q->qmutex);
    return msg;
}


net_err_t fix_queue_send(fix_queue_t *q, void *msg, int time_out) {
    sys_mutex_lock(q->qmutex);
    LOG_DEBUG("[send]msg queue size:%d, msg queue cnt:%d\n", q->size, q->cnt);
    if (q->cnt >= q->size) {
        if (time_out == 0) {
            LOG_DEBUG("msg queue full\n");
            sys_mutex_unlock(q->qmutex);
            return NET_ERR_FULL;
        }
        q->send_wait_cnt++;
        sys_mutex_unlock(q->qmutex);
        if (sys_event_wait(q->write_enable, time_out) < 0) {
            sys_mutex_lock(q->qmutex);
            q->send_wait_cnt--;
            LOG_DEBUG("msg queue write timeout\n");
            sys_mutex_unlock(q->qmutex);
            return NET_ERR_TIMEOUT;
        }
        sys_mutex_lock(q->qmutex);
        q->send_wait_cnt--;
        LOG_DEBUG("[send prev]msg cnt = %d\n", q->cnt);
    }
    q->buf[q->in++] = msg;
    if (q->in >= q->size) q->in = 0;
    q->cnt++;
    LOG_DEBUG("current recv wait cnt:%d\n", q->recv_wait_cnt);
    if (q->recv_wait_cnt) sys_event_notify(q->read_enable);
    sys_mutex_unlock(q->qmutex);
    return NET_ERR_OK;
}